#include <vector>
#include <math.h>
#include <cmath>
#include <iostream>
#include "trapezoidal_ed.h"
using namespace std;

// // define the parameters of the curve
// struct CurveParam
// {
//     double aa_max;  // the maximum accelerattion acceleration
//     double acc_max; // the maximum acceleration
//     double t_acc;   // the period in which acceleration is increasing
//     double t_keep;  // the period in which acceleration remains unchanged, but not zero 
//     double t_const; // the period in which acceleration is zero

//     inline bool operator==(const CurveParam &c_param) const
//     {
//         return (c_param.aa_max == aa_max && c_param.t_acc == t_acc && c_param.t_keep == t_keep && c_param.t_const == t_const);
//     }
// };

// define the parameters of the input
// struct InParam
// {
//     double acc_max;     // the maximum acceleration
//     double vel_max;     // the maximum velocity
//     double aa_max;      // the maximum accelerattion acceleration

//     double theta_start;     // the starting angle
//     double theta_end;       // the endding angle

//     inline bool operator==(const InParam &in_param) const
//     {
//         return (in_param.acc_max == acc_max && in_param.vel_max == vel_max && in_param.aa_max == aa_max && in_param.acc_max == acc_max && in_param.theta_start == theta_start && in_param.theta_end == theta_end);
//     }
// };

void Trapezoidal(InParam in_param, CurveParam &c_param);

double compute(double time, CurveParam c_param);

double TimeTool(double time, double t_1, double t_2);


// compute the relative time in each section of the curve
// input: time - the time needed to be calculated
//        t_1 - the lower bound of the section
//        t_2 - the upper bound of the section
// output: ret - the relative time in the section
// description: if time > t_2, then return the time of the whole setion,
//              if t_2 > time > t_1, then return (time - t_1)
//              if t_1 > time, then time has not reach the section, thus return 0
double TimeTool(double time, double t_1, double t_2)
{
    double ret = 0;
    if (time > t_2)
    {
        ret = t_2 - t_1;
    }
    else if (time > t_1)
    {
        ret = time - t_1;
    }
    
    return ret;
}

// compute the rotation angle given time and the curve
// input: time - the time when the angle needed to be computed
//        c_param - the parameters given by the curve
// output: ret - the rotation angle, which is integrated over all the sections
// description: when the parameters of the curve is determined, then the seven moment (t1,...,t7) on the curve is determined
//              then wo calculate the relative time in each section and then the rotation angle in each section
double compute(double time, CurveParam c_param)
{
    // the parameters given by the curve
    double t_acc = c_param.t_acc;
    double t_keep = c_param.t_keep;
    double t_const = c_param.t_const;
    double aa_max = c_param.aa_max;
    double acc_max = c_param.acc_max;
    
    // each moment on the curve determined by the parameters of the curve
    double t0 = 0;
    double t1 = t0 + t_acc;
    double t2 = t1 + t_keep;
    double t3 = t2 + t_acc;
    double t4 = t3 + t_const;
    double t5 = t4 + t_acc;
    double t6 = t5 + t_keep;
    double t7 = t6 + t_acc;
    double t = 0;

    double ret = 0;
 
    t = TimeTool(time, t0, t1);
    ret += 0.5 * aa_max * pow(t, 2);

    t = TimeTool(time, t1, t2);
    ret += acc_max * t;

    t = TimeTool(time, t2, t3);
    ret += aa_max * t_acc * t - 0.5 * aa_max * pow(t, 2);

    t = TimeTool(time, t3, t4);
    ret += 0;

    t = TimeTool(time, t4, t5);
    ret += -0.5 * aa_max * pow(t, 2);

    t = TimeTool(time, t5, t6);
    ret += -acc_max * t; 

    t = TimeTool(time, t6, t7);
    ret += -aa_max * t_acc * t + 0.5 * aa_max * pow(t, 2); 
    
    return ret;
    
}

void Trapezoidal(InParam in_param, CurveParam &c_param)
{
    double phi = in_param.theta_end - in_param.theta_start;

    // the maximum velocity when the maximum acceleration is reached
    double vel_max = in_param.acc_max * in_param.acc_max / (in_param.aa_max);
    // the rotation angle threshold
    double phi_1 = 2 * in_param.aa_max * pow(in_param.vel_max / in_param.aa_max, 1.5);
    double phi_2 = 2 * pow(in_param.acc_max, 3) / pow(in_param.aa_max, 2);
    double phi_3 = in_param.acc_max * in_param.vel_max / in_param.aa_max + pow(in_param.vel_max, 2) / in_param.acc_max;

    c_param.aa_max = in_param.aa_max;
    c_param.acc_max = in_param.acc_max;

    if (vel_max > in_param.vel_max)
    {
        if (phi < phi_1)
        {
            c_param.t_acc = pow(phi / (in_param.aa_max * 2), 1.0 / 3);
            c_param.t_keep = 0;
            c_param.t_const = 0;
        }
        else
        {
            c_param.t_acc = sqrt(in_param.vel_max / in_param.aa_max);
            c_param.t_keep = 0;
            c_param.t_const = (phi - 2 * in_param.aa_max * pow(c_param.t_acc, 3)) / (in_param.vel_max);
        }
    }
    else
    {
        if (phi < phi_2)
        {
            c_param.t_acc = pow(phi / (2 * in_param.aa_max), 1.0/3);
            c_param.t_keep = 0;
            c_param.t_const = 0;
        }
        else if (phi < phi_3)
        {
            c_param.t_acc = (in_param.acc_max) / (in_param.aa_max);
            c_param.t_keep = (-3 * c_param.t_acc + sqrt(pow(c_param.t_acc, 2) + 4 * phi / in_param.acc_max)) / 2;
            c_param.t_const = 0;
        }
        else
        {
            c_param.t_acc = (in_param.acc_max) / (in_param.aa_max);
            c_param.t_keep = (in_param.vel_max - in_param.acc_max * c_param.t_acc) / (in_param.acc_max);
            c_param.t_const = (phi - in_param.acc_max * (pow(c_param.t_keep, 2) + 2 * pow(c_param.t_acc, 2) + 3 * c_param.t_keep * c_param.t_acc)) / (in_param.vel_max);
        }
    }
}
